
#ifndef ROBOT_INVERSEKINEMATICS
#define ROBOT_INVERSEKINEMATICS

/** InverseKinematics
 * Calculate inverse kinematics on the fly
 * Specific to the 4 joint arm defined in robot.xml with definable link lengths:
 * l1 to l5. Note: l2 is the link offset from link 1
 */

#include "termcolors.hpp"
#include "robotStruct.hpp"
#include <openrave-core.h>
#include <vector>
#include <iostream>

// Return Codes for solver:
#define IK_SUCCESS 0
#define IK_WARN_NON_DEXTROUS 1
#define IK_ERROR_UNREACHABLE 2

// Specify behaviour when the endangle is fit to a solution:
// Should the solver edit the endangle in place or change a copy
#define IK_ENDANGLE_MODIFY_PARAM 3
#define IK_ENDANGLE_MODIFY_COPY 4

class InverseKinematics {
 public:
  InverseKinematics(RobotModel* rm);
  ~InverseKinematics();
  
  int solve(jvec4d* angles, cvec4d* end, int editMode = IK_ENDANGLE_MODIFY_COPY);
  
 private:
  double l1_, l2_, l3_, l4_, l5_;
};

inline InverseKinematics::InverseKinematics(RobotModel* rm) {
  l1_ = rm->l1;
  l2_ = rm->l2;
  l3_ = rm->l3;
  l4_ = rm->l4;
  l5_ = rm->l5;
}

inline InverseKinematics::~InverseKinematics() {
  // Destructor
}

/** solve:
 * 
 * Solve inverse kinematics from end effector coords: endx, endy, endz
 * and end effector angle: endangle
 * populates angles vector (must be size 4)
 * 
 * returns int errorcode:
 *  0: success
 *  1: limited endangle - reachable but not dextrous location: endangle ignored
 *  2: failure - unreachable location: endx, endy, endz
 */
inline int InverseKinematics::solve(jvec4d* angles, cvec4d* end, int editMode) {
  // Convert to birds-eye angle and radius:
  angles->t1 = atan2(end->y, end->x);
  double endr = sqrt(end->y*end->y + end->x*end->x);
  double reach = sqrt(endr*endr + ( end->z-l1_ )*( end->z-l1_ ));
  double maxDist = l2_+l3_+l4_+l5_;
  
  // If all the links are outstretched can we reach it:
  // Note ignore l2 so reaching up is satisfied?
  if(reach > maxDist) {
    std::cout << FONT_ERROR << "Needs to reach " << reach << "radially\n";
    std::cout << "Reachable area maximum radius is " << maxDist << "\n";
    std::cout << "IK: unreachable location\n" << FONT_RESET;
    return IK_ERROR_UNREACHABLE;  // unreachable
  }
  
  // Find fourth joint location in radial space: (r,z)
  double r4 = endr - l5_*cos(end->phi);
  double z4 = end->z + l5_*sin(end->phi);

  // Find solution to elbow:
  double dx = r4 - l2_;
  double dy = z4 - l1_;
  double dlen = sqrt(dx*dx + dy*dy);
  double inAng1, inAng2, inAng3;
  // Check whether a solution exists for this endangle: 
  if(dlen < l3_+l4_) {
    // Solution exists:
    // Find inner angles to the triangle formed by l3 and l4 and dlen:
    // inAng1 is angle between dlen and l3:
    inAng1 = acos( (dlen*dlen + l3_*l3_ - l4_*l4_)/(2*dlen*l3_) );
    // inAng2 is angle between l3 and l4:
    inAng2 = acos( (l3_*l3_ + l4_*l4_ - dlen*dlen)/(2*l3_*l4_) );
    // inAng3 is the angle between horizontal and dlen:
    inAng3 = atan2(dy, dx);
  
    // always return solution #1 of 2 for now:
    angles->t2 = inAng3 + inAng1;
    //angles->t2 = inAng3 - inAng1;
    angles->t3 = inAng2 - M_PI;
    
    angles->t4 = -(end->phi + angles->t2 + angles->t3);
    
    // Ensure we dont obstruct the base:
    if(angles->t2 > M_PI || angles->t2 < -M_PI/2) {
      std::cout << FONT_ERROR << "IK: Solution obstructs base!\nangles: ("
                << angles->t1 << "," << angles->t2 << "," << angles->t3
                << "," << angles->t4
                << ")\nto reach pos: (" << end->x << "," << end->y << ","
                << end->z << ")\n" << FONT_RESET;
      return IK_ERROR_UNREACHABLE;
    }
    
    return IK_SUCCESS;
  } else {
    // Find rejected endangle solution for reachable but not dextrous space:
    std::cout << FONT_WARN << "IK: Non-dextrous solution\n";
    
    dx = endr - l2_;
    dy = end->z - l1_;
    dlen = sqrt(dx*dx + dy*dy);
    // Find inner angles to the triangle formed by l3+l4 and l5 and dlen:
    // inAng1 is angle between dlen and l3:
    inAng1 = acos( ((l3_+l4_)*(l3_+l4_) + dlen*dlen - l5_*l5_)/(2*dlen*(l3_+l4_)) );
    // inAng3 is the angle between horizontal and dlen:
    inAng3 = atan2(dy, dx);
    
    // Redefine endangle:
    double endangle = asin( (l3_+l4_)*sin(inAng1)/l5_ );
    std::cout << "Forced end angle to " << endangle*180/M_PI << "\n" << FONT_RESET;
    
    if(editMode == IK_ENDANGLE_MODIFY_PARAM) end->phi = endangle;
    
    // always return solution #1 of 2 for now:
    angles->t2 = inAng3 + inAng1;
    //angles->t2 = inAng3 - inAng1;
    angles->t3 = 0.0;
    
    angles->t4 = -(endangle + angles->t2);
    
    if(abs(angles->t4) > M_PI) {
      std::cout << FONT_ERROR << "Unreachable location\n" << FONT_RESET;
      return IK_ERROR_UNREACHABLE;
    }
    
    return IK_WARN_NON_DEXTROUS;
  }
}

#endif
